
function [c, ceq, Gc, Gceq] = Bcr4bp_ConsEMRot2D_Coe(vars , aux)
%
% 构造约束和梯度
% 1) 2D
% 2) EMRot
%
% 作者：张晨
% 单位：中科院空间应用工程与技术中心
% 时间：2022年02月22日
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

c = [];
ceq = [];
Gc = [];
Gceq = [];

% DRO入轨脉冲 (VU)
dvMag = aux.dvMag;

%% ------------------------------------------- 提取变量 ------------------------------------------
% 变量数
nVar = size(vars , 1);

% 节点数
nNode = aux.nNode;

% 状态矩阵
xx_mtx = reshape(vars(1 : 4 * nNode) , 4 , nNode); % 状态

% 开始时间
tau_1 = vars(4 * nNode + 1);

% 结束时间
tau_f = vars(4 * nNode + 2);

% DRO相位角
sigma = vars(4 * nNode + 3);

% % DRO入轨点脉冲 (VU)
% dvMag = unifrnd(dvBound(1) , dvBound(2) , 1);

% dro入轨点状态 (LU , VU)
periOrb_x = interp1(aux.periOrb_tt , aux.periOrb_xx , sigma * aux.periOrb_P , 'spline')';
x_dro = periOrb_x(1);
y_dro = periOrb_x(2);
vx_dro = periOrb_x(3);
vy_dro = periOrb_x(4);

pDro_pt = Bcr4bp_EqmEMRot2D(tau_f , periOrb_x , aux);

% 计算卫星入轨状态
xx_target = [x_dro;
    y_dro;
    vx_dro + dvMag * vx_dro / sqrt(vx_dro^2 + vy_dro^2);
    vy_dro + dvMag * vy_dro / sqrt(vx_dro^2 + vy_dro^2)];

% 时间向量
tau_vec = linspace(tau_1 , tau_f , nNode)';

%% -------------------------------------- 输出预分配 -------------------------------------
% 无不等式约束
c = [];
Gc = [];

% 状态连接约束
stateCeq = zeros(4 * (nNode - 1) , 1);
stateGceq = zeros(4 * (nNode - 1) , nVar);
stateCeqCount = 0;

% 保存每段时间和状态
seg_tt_all = [];
seg_xx_all = [];

% 构造多步打靶约束
for jLoop = 1 : (nNode - 1)

    % 提取单轨道段两端的时刻
    tau_k = tau_vec(jLoop);
    tau_kp1 = tau_vec(jLoop + 1);

    % 提取单轨道段两端的状态变量
    xx_tauk = xx_mtx(: , jLoop);
    xx_taukp1 = xx_mtx(: , jLoop + 1);

    % 数值积分
    phi0 = eye(4);
    options = odeset('Reltol' , aux.tol , 'AbsTol' , aux.tol);
    [seg_tt , seg_xx] = ode113(@Bcr4bp_StmEMRot2D , [tau_k , tau_kp1] , [xx_tauk ; phi0(:)] , options , aux);

    % 保存全部轨道
    seg_tt_all = [seg_tt_all ; seg_tt];
    seg_xx_all = [seg_xx_all ; seg_xx];

    % 积分终端状态
    xx_taukp1_ = seg_xx(end , 1 : 4)';

    % 状态转移矩阵
    phi_tauk_taukp1 = reshape(seg_xx(end , 5 : 20) , 4 , 4);

    % 状态连接约束和梯度
    [stateCeqN_temp , stateCeq_temp , stateGceq_temp] = Bcr4bp_StateConsEMRot2D(tau_k , xx_tauk , tau_kp1 , xx_taukp1 , ...
        xx_taukp1_ , phi_tauk_taukp1 , nVar , nNode , jLoop , aux);

    % 更新
    stateCeq(stateCeqCount + 1 : stateCeqCount + stateCeqN_temp , :) = stateCeq_temp;
    stateGceq(stateCeqCount + 1 : stateCeqCount + stateCeqN_temp , :) = stateGceq_temp;
    stateCeqCount = stateCeqCount + stateCeqN_temp;

end

%% ---------------------------------------------- dep 约束 ------------------------------------------
mu = aux.EMRot.mu;

x1 = xx_mtx(1 , 1);
y1 = xx_mtx(2 , 1);
vx1 = xx_mtx(3 , 1);
vy1 = xx_mtx(4 , 1);

% LEO约束
depCeq = [sqrt((x1 + mu)^2 + y1^2) - (aux.dim.rMagLEO / aux.dim.EMRot_l);
    (x1 + mu) * vx1 + y1 * vy1];
depGceq = zeros(2 , nVar);
depGceq(: , 1 : 4) = [(2*mu + 2*x1)/(2*((mu + x1)^2 + y1^2)^(1/2)), y1/((mu + x1)^2 + y1^2)^(1/2),0,0;
    vx1, vy1, mu + x1, y1];

%% ---------------------------------------------- arr 约束 ------------------------------------------
xn = xx_mtx(1 , end);
yn = xx_mtx(2 , end);
vxn = xx_mtx(3 , end);
vyn = xx_mtx(4 , end);

% LLO约束
arrCeq = [xn - xx_target(1);
    yn - xx_target(2);
    vxn - xx_target(3);
    vyn - xx_target(4)];

% 【测试】
% syms x_dro y_dro vx_dro vy_dro dvMag
% XX = [x_dro; 
%     y_dro; 
%     vx_dro + dvMag * vx_dro / sqrt(vx_dro^2 + vy_dro^2);
%     vy_dro + dvMag * vy_dro / sqrt(vx_dro^2 + vy_dro^2)];
% simplify(jacobian(XX , [x_dro ; y_dro ; vx_dro ; vy_dro]))

part1 = [1, 0, 0, 0;
    0, 1, 0, 0;
    0, 0, ((vx_dro^2 + vy_dro^2)^(3/2) + dvMag*vy_dro^2)/(vx_dro^2 + vy_dro^2)^(3/2), -(dvMag*vx_dro*vy_dro)/(vx_dro^2 + vy_dro^2)^(3/2);
    0, 0,  -(dvMag*vx_dro*vy_dro)/(vx_dro^2 + vy_dro^2)^(3/2), ((vx_dro^2 + vy_dro^2)^(3/2) + dvMag*vx_dro^2)/(vx_dro^2 + vy_dro^2)^(3/2)];
part2 = pDro_pt;
part3 = aux.periOrb_P;

arrGceq = zeros(4 , nVar);
arrGceq(: , 4 * (nNode - 1) + 1 : 4 * nNode) = eye(4);
arrGceq(: , 4 * nNode + 3) = - part1 * part2 * part3;

%% 拼约束和梯度
% 约束拼接
ceq = [stateCeq ; depCeq ; arrCeq];

% 梯度拼接
Gceq = [stateGceq ; depGceq ; arrGceq];

% 构造约束和梯度
Gceq = Gceq';
Gc = Gc';

end


function [ceqNum , ceq , Gceq] = Bcr4bp_StateConsEMRot2D(tau_k , xx_tauk , tau_kp1 , xx_taukp1 , ...
    xx_taukp1_ , phi_tauk_taukp1 , nVar , nNode , jLoop , aux)
%
% 构造状态连接约束
%
% 注意：
% 如果xxkp1是'dv'，则约束为r1f - r20
% 否则都为x1f - x20
%
% 作者：张晨
% 邮箱：chenzhang@csu.ac.cn
% 单位：中国科学院空间应用工程与技术中心，空间探索室
% 时间：2021年08月25日
%%%%%%%%%%%%%%%%%%%%%%%%

% ---------------------------------------------------------------------------
% tauk动力学
dxx_tauk = Bcr4bp_EqmEMRot2D(tau_k , xx_tauk , aux);

% taukp1_动力学
dxx_taukp1_ = Bcr4bp_EqmEMRot2D(tau_kp1 , xx_taukp1_ , aux);

% 模块
dtauj_dtau1 = 1 - (jLoop - 1) / (nNode - 1);
dtaujp1_dtau1 = 1 - jLoop / (nNode - 1);
dtauj_dtaun = (jLoop - 1) / (nNode - 1);
dtaujp1_dtaun = jLoop / (nNode - 1);

% ---------------------------------------------------------------------------
% 连接约束数
ceqNum = 4;

% 约束
ceq = (xx_taukp1_ - xx_taukp1);

% 梯度预分配
Gceq = zeros(4 , nVar);

% --------------- 状态连接约束（梯度） -------------
% wrt xx_tauk
Gceq(: , 4 * (jLoop - 1) + 1 : 4 * jLoop) = phi_tauk_taukp1;

% wrt xxkp1
Gceq(: , 4 * jLoop + 1 : 4 * (jLoop + 1)) = - eye(4);

try
    % 状态连接约束 wrt tau_1
    Gceq(: , 4 * nNode + 1) = - phi_tauk_taukp1 * dxx_tauk * dtauj_dtau1 + dxx_taukp1_ * dtaujp1_dtau1;
catch
    dd = [];
end

% 状态连接约束 wrt tau_n
Gceq(: , 4 * nNode + 2) = - phi_tauk_taukp1 * dxx_tauk * dtauj_dtaun + dxx_taukp1_ * dtaujp1_dtaun;

end

